#include "app_porting.h"

//#define KALMAN_DEBUG_ENABLE

typedef double KALMAN_DTYPE;

#if 1
#define KALMAN_Q_VALUE 0.0123
#define KALMAN_R_VALUE 16.7415926
#define KALMAN_ADJUST 0.0
#define KALMAN_P_LAST 0.45
#else
#define KALMAN_Q_VALUE 0.018
#define KALMAN_R_VALUE 0.542
#define KALMAN_ADJUST 0.0
#define KALMAN_P_LAST 0.02
#endif

/*卡尔曼滤波需要设置4个值, 其余均可自动调整
Q,R,x_last,p_last
*/
/*系统噪声协方差, 值越大->精度越低->速度越快*/
static KALMAN_DTYPE Q;
/*测量噪声协方差, 值越大->精度越高->速度越慢*/
static KALMAN_DTYPE R;
/*先前最优值, 初始值不能为0!!!*/
static KALMAN_DTYPE x_last;
/*先前系统协方差, 初始值不能为0!!!*/
static KALMAN_DTYPE p_last;

/*调整值, 设定调整值的原因是加快调整速度, 可以不加调整变量*/
static float adjust; 

void kalman_init(long measure)
{
	Q = KALMAN_Q_VALUE;
	R = KALMAN_R_VALUE;
	adjust=KALMAN_ADJUST; 

#if 1
	if(0==measure){
		measure=1;
	}
#else
	if(fabs(measure)<0.01){
		measure+=0.01;
	}
#endif

	/*先前最优值, 初始值不能为0!!!*/	
	x_last=measure;
	
	/*先前系统协方差, 初始值不能为0!!!*/
	p_last = KALMAN_P_LAST;
#ifdef KALMAN_DEBUG_ENABLE	
	printf("[%s](%d)x_last=%f\r\n",__FUNCTION__,__LINE__,p_last);
#endif
}

long kalman_fillter(long measure)
{	
	/*当前估计值, */
	KALMAN_DTYPE x_mid;
	/*卡尔曼增益*/
	KALMAN_DTYPE kg;
	/*当前系统最优值*/
	KALMAN_DTYPE x_now;
	/*当前系统协方差*/
	KALMAN_DTYPE p_mid;	

	/*.1.当前估计值=先前最优值+调整值
	X(k|k-1)=A X(k-1|k-1)+B U(k)*/
	x_mid = x_last+adjust;	
	
	/*.2.当前系统协方差=先前系统协方差+系统噪声协方差
	P(k|k-1)=A P(k-1|k-1) A’+Q
	*/
	p_mid = p_last + Q;	
	
	/*.3.卡尔曼增益=当前系统协方差/(当前系统协方差+测量噪声协方差)
	Kg(k)= P(k|k-1) H’ / (H P(k|k-1) H’ + R)
	*/
	kg = p_mid / (p_mid + R);

	/*.4.当前系统最优值=当前系统估计值+卡尔曼增益*（测量值-当前系统估计值）
	X(k|k)= X(k|k-1)+Kg(k) (Z(k)-H X(k|k-1))
	*/
	x_now = x_mid + kg*(measure - x_mid);//估计出的最优值	
	
	/*.5.先前系统噪声协方差=（1-卡尔曼增益）x当前系统协方差
	P(k|k)=（I-Kg(k) H）P(k|k-1)
	*/
	p_last = (1 - kg)*p_mid;//最优值对应的协方差	
	
	/*递归处理当前系统最优值*/
	x_last = x_now;
#ifdef KALMAN_DEBUG_ENABLE	
	printf("[%s](%d)x_last=%f\r\n",__FUNCTION__,__LINE__,p_last);
#endif	
	return (long)x_now;	
}

#ifdef TEST_ENABLE
void kalman_test(void)
{
	float x_now;
	float z_real = 0.56;
	float z_measure;
	float summerror_kalman = 0;
	float summerror_measure = 0;
	int i;
	
	kalman_init(z_real + rand()*0.03);
	for (i = 0; i < 20;i++)
	{
		z_measure = z_real + rand()*0.03;//测量值
		x_now=kalman_fillter(z_measure);

		printf("Real position:%6.3f\n", z_real);
		printf("Measure position:%6.3f [diff:%.3f]\n", z_measure, fabs(z_real - z_measure));
		printf("Kalman position: %6.3f [diff:%.3f]\n", x_now, fabs(z_real - x_now));
		printf("\n\n");
		summerror_kalman += fabs(z_real - x_now);
		summerror_measure += fabs(z_real - z_measure);


	}
	printf("总体测量误差      :%f\n", summerror_measure);
	printf("总体卡尔曼滤波误差:%f\n", summerror_kalman);
	printf("卡尔曼误差所占比例:%d%%\n", 100 - (int)((summerror_kalman / summerror_measure) * 100));

	getchar();

}
#endif

